//
//  GL_SteeringBehavior.m
//  RecipeCollection02
//
//  Created by Louis CHU on 02/03/12.
//  Copyright (c) 2012 Nathan Burba. All rights reserved.
//

#import "GL_SteeringBehavior.h"
#import "GL_Vehicle.h"

#import "GL_TransformationCoordonation.h"


static GL_SteeringBehavior * sDefaultSteeringBehavior;
@implementation GL_SteeringBehavior
+ (GL_SteeringBehavior *) defaultSteeringBehavior
{
    if (!sDefaultSteeringBehavior) {
        sDefaultSteeringBehavior = [GL_SteeringBehavior new];
        
    }
    return sDefaultSteeringBehavior;
}

- (b2Vec2) agent: (GL_Vehicle *)agent seekTarget: (b2Vec2) target;
{
    b2Vec2 desiredVelocity = (target - agent.position);
    desiredVelocity *= agent.maxSpeed;
    return desiredVelocity - agent.velocity;
}

- (b2Vec2) agent: (GL_Vehicle *) agent avoidObstacle: (GL_BaseEntity *) obstacle
{
    
    b2Vec2 localPosObObstacle = [GL_TransformationCoordonation transformTarget:obstacle.position  axisDirection:agent.velocity fromOrigin:b2Vec2(0,0) toOrigin:agent.position];
    float32 detectionBoxLength = agent.detectionBoxLength;
    float32 multiplier = 1.0f + (detectionBoxLength - localPosObObstacle.x)/detectionBoxLength;
    
    b2Vec2 steeringForce = b2Vec2(0,0);
    steeringForce.y = (obstacle.bRadius - localPosObObstacle.y) * multiplier;
    
    const float32 brakingWeight = 0.2f;
    steeringForce.x = (obstacle.bRadius - localPosObObstacle.x) * brakingWeight;
    
    b2Vec2 fromOriginInNewCoord = [GL_TransformationCoordonation transformTarget:b2Vec2(0.f,0.f)  axisDirection:agent.velocity fromOrigin:b2Vec2(0.f,0.f)  toOrigin:agent.position];
    b2Vec2 normalizedSteeringForce = [GL_TransformationCoordonation transformTarget:steeringForce  axisDirection:b2Vec2(agent.velocity.x,-agent.velocity.y) fromOrigin:b2Vec2(0,0) toOrigin:fromOriginInNewCoord];
    
    return normalizedSteeringForce;
}




@end
